#include "stdafx.h"
#include <iostream>

void GrabRobModel_Test()
{
	cout<< "GrabRobModel_Test start."<< endl;
	ColumnVector q(3);q.SetZero();
	ColumnVector q2(3);q2.SetZero();

	ColumnVector qd(3);qd.SetZero();
	ColumnVector qd2(3);qd2.SetZero();

	ColumnVector qdd(3);qdd.SetZero();
	ColumnVector qdd2(3);qdd2.SetZero();

	ColumnVector qMax(3);
	ColumnVector qMin(3);
	ColumnVector qMax2(3);
	ColumnVector qMin2(3);
	qMax[0] = PI;
	qMin[0] = -PI;
	qMax[1] = PI;
	qMin[1] = -PI;
	qMax[2] = 2*PI;
	qMin[2] = -2*PI;


	GrabRob_3_RRR arm = GrabRob_3_RRR();

	q[0] = PI/4;
	q[1] = PI/4;
	q[2] = PI/4;

	qd[0] = 1;
	qd[1] = 2;
	qd[2] = 3;

	qdd[0] = 1;
	qdd[1] = 2;
	qdd[2] = 3;

	//set and get joint values
	arm.Set_JointValues(q);
	q2 = arm.Get_JointValues();
	if(q2 != q)
	{
		cout<< "Set_JointValues and Get_JointValues failed."<< endl;
	}

	//set and get joint values max
	arm.Set_JointValuesMax(qMax);
	qMax2 = arm.Get_JointValuesMax();
	if(qMax2 != qMax)
	{
		cout<< "Set_JointValuesMax and Get_JointValuesMax failed."<< endl;
	}

	//set and get joint values min
	arm.Set_JointValuesMin(qMin);
	qMin2 = arm.Get_JointValuesMin();
	if(qMin2 != qMin)
	{
		cout<< "Set_JointValuesMin and Get_JointValuesMin failed."<< endl;
	}

	//fk pose and inv pose.
	Frame3 pose = arm.ForwardKine_Pose(q);
	vector<ColumnVector> q2List = vector<ColumnVector>();
	bool ret = arm.InverseKine_Pose(pose,q2List);
	if(ret == false)
	{
		cout<< "ForwardKine_Pose and InverseKine_Pose failed."<< endl;
	}
	else
	{

		bool found = false;
		for(int i=0; i < (int)q2List.size();i++)
		{
			cout<< q2List[i][0] << " " << q2List[i][1] << " " << q2List[i][2] << endl;
			if(q == q2List[i])
			{
				found = true;
			}
		}
		if(found == false)
			cout << "ForwardKine_Pose and InverseKine_Pose failed."<< endl;
	}

	//fk vel and inv vel.
	Velocity vel = arm.ForwardKine_Vel(q,qd);
	ret = arm.InverseKine_Vel(q,vel,qd2);
	if((ret == false) || (qd2 != qd))
	{
		cout<< "ForwardKine_Vel and InverseKine_Vel failed."<< endl;
	}

	//fk acc and inv acc.
	Acceleration acc = arm.ForwardKine_Acc(q,qd,qdd);
	ret = arm.InverseKine_Acc(q,qd, acc, qdd2);
	if((ret == false) || (qdd2 != qdd))
	{
		cout<< "ForwardKine_Acc and InverseKine_Acc failed."<< endl;
	}


	cout<< "GrabRobModel_Test end."<< endl << endl;
}